#include "HavokWorld.h"



HavokWorld* HavokWorld::theHavokWorld = NULL;

HavokWorld::HavokWorld(void)
{
	theHavokWorld = HK_NULL;
	threadPool = HK_NULL;
	jobQueue = HK_NULL;
	vdb = HK_NULL;
	context = HK_NULL;
	timestep = HK_NULL;
}

void HavokWorld::Init()
{
	timestep = (1.0f / UPDATESPERSECOND*2);

	SetupJobQueue();
	SetupHavokWorld();

}


void HavokWorld::Update()
{
	// clean up dead objects

	physicsWorld->stepMultithreaded( jobQueue, threadPool, timestep );

	// Step the visual debugger. We first synchronize the timer data
	context->syncTimers( threadPool );
	vdb->step();

	//// Clear accumulated timer data in this thread and all slave threads
	hkMonitorStream::getInstance().reset();
	threadPool->clearTimerData();
}

void HavokWorld::SetupJobQueue()
{
	// Initialize the base system including our memory system
	hkMemoryRouter* memoryRouter = hkMemoryInitUtil::initDefault();
	hkBaseSystem::init( memoryRouter, errorReport );

	// We can cap the number of threads used - here we use the maximum for whatever multithreaded platform we are running on. This variable is
	// set in the following code sections.
	int totalNumThreadsUsed;

	// Get the number of physical threads available on the system
	hkHardwareInfo hwInfo;
	hkGetHardwareInfo(hwInfo);
	totalNumThreadsUsed = hwInfo.m_numThreads;

	// We use one less than this for our thread pool, because we must also use this thread for our simulation
	hkCpuJobThreadPoolCinfo threadPoolCinfo;
	threadPoolCinfo.m_numThreads = totalNumThreadsUsed - 1;

	// This line enables timers collection, by allocating 200 Kb per thread.  If you leave this at its default (0),
	// timer collection will not be enabled.
	threadPoolCinfo.m_timerBufferPerThreadAllocation = 200000;
	threadPool = new hkCpuJobThreadPool( threadPoolCinfo );

	// We also need to create a Job queue. This job queue will be used by all Havok modules to run multithreaded work.
	// Here we only use it for physics.
	hkJobQueueCinfo info;
	info.m_jobQueueHwSetup.m_numCpuThreads = totalNumThreadsUsed;
	jobQueue = new hkJobQueue(info);

	// Monitors have been enabled for thread pool threads already (see above comment).
	hkMonitorStream::getInstance().resize(200000);
}

void HavokWorld::SetupHavokWorld()
{
	// The world cinfo contains global simulation parameters, including gravity, solver settings etc.
	hkpWorldCinfo worldInfo;

	// Set the simulation type of the world to multi-threaded.
	worldInfo.m_simulationType = hkpWorldCinfo::SIMULATION_TYPE_MULTITHREADED;
	worldInfo.m_gravity = hkVector4(0,-9.8,0,0);

	// Flag objects that fall "out of the world" to be automatically removed - just necessary for this physics scene
	worldInfo.m_broadPhaseBorderBehaviour = hkpWorldCinfo::BROADPHASE_BORDER_REMOVE_ENTITY;

	physicsWorld = new hkpWorld(worldInfo);

	// Disable deactivation, so that you can view timers in the VDB. This should not be done in your game.
	physicsWorld->m_wantDeactivation = false;


	// When the simulation type is SIMULATION_TYPE_MULTITHREADED, in the debug build, the sdk performs checks
	// to make sure only one thread is modifying the world at once to prevent multithreaded bugs. Each thread
	// must call markForRead / markForWrite before it modifies the world to enable these checks.
	physicsWorld->markForWrite();


	// Register all collision agents, even though only box - box will be used in this particular example.
	// It's important to register collision agents before adding any entities to the world.
	hkpAgentRegisterUtil::registerAllAgents( physicsWorld->getCollisionDispatcher() );

	// We need to register all modules we will be running multi-threaded with the job queue
	physicsWorld->registerWithJobQueue( jobQueue );

	
}


void HavokWorld::StartVisualDebugger()
{
	hkArray<hkProcessContext*> contexts;

	// The visual debugger so we can connect remotely to the simulation
	// The context must exist beyond the use of the VDB instance, and you can make
	// whatever contexts you like for your own viewer types.
	context = new hkpPhysicsContext();
	hkpPhysicsContext::registerAllPhysicsProcesses(); // all the physics viewers
	context->addWorld(physicsWorld); // add the physics world so the viewers can see it
	contexts.pushBack(context);

	// Now we have finished modifying the world, release our write marker.
	physicsWorld->unmarkForWrite();

	vdb = new hkVisualDebugger(contexts);
	vdb->serve();

}

void HavokWorld::Cleanup()
{
	//
	// Clean up physics and graphics
	//
	physicsWorld->markForWrite();
	physicsWorld->removeReference();

	vdb->removeReference();

	// Contexts are not reference counted at the base class level by the VDB as
	// they are just interfaces really. So only delete the context after you have
	// finished using the VDB.
	context->removeReference();

	delete jobQueue;

	// Clean up the thread pool

	threadPool->removeReference();

	hkBaseSystem::quit();
	hkMemoryInitUtil::quit();
}

void HavokWorld::Lock()
{
	physicsWorld->lock();
}

void HavokWorld::Unlock()
{
	physicsWorld->unlock();
}

hkReal HavokWorld::GetTimestep()
{
	return timestep;
}

void HavokWorld::RemoveFromWorld( hkpRigidBody* body )
{
	//ToBeRemoved.push_back(body);
	physicsWorld->removeEntity(body);
}

